
class PID:
  def __init__(self, P, I, D):
    self.Kp = P
    self.Ki = I
    self.Kd = D
    self.sample_time = 0.00
    self.dt = 0.3
    self.clear()
  def clear(self):
    self.error = 0.0
    self.PTerm = 0.0
    self.ITerm = 0.0
    self.DTerm = 0.0
    self.last_error = 0.0
    self.output = 0.0
  def update(self, error):
    delta_error = error - self.last_error
    self.PTerm = self.Kp * error#比例
    self.DTerm = delta_error / self.dt  #微分 

    self.last_error = error
    self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm)
    return self.output

    
  def setKp(self, proportional_gain):
    self.Kp = proportional_gain
  def setKi(self, integral_gain):
    self.Ki = integral_gain
  def setKd(self, derivative_gain):
    self.Kd = derivative_gain
  def setSampleTime(self, sample_time):
    self.sample_time = sample_time